/*    

    bobbietalk - a visual programming environment for toy robotics

    Copyright (C) 2013  Roberto Tiella

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.

*/
package bobbietalk.simulator;

import bobbietalk.simulator.sensors.Sensor;
import bobbietalk.simulator.sensors.UltrasonicSensor;
import bobbietalk.simulator.world.World;
import java.awt.Shape;
import java.awt.geom.AffineTransform;
import java.awt.geom.GeneralPath;
import java.awt.geom.PathIterator;
import java.awt.geom.Rectangle2D;
import java.util.Observable;
import java.util.Observer;
import javax.vecmath.Vector3d;

/**
 *
 * @author roberto
 */
public class NxtRobot extends AbstractSimpleRobot {

    private UltrasonicSensor usens = new UltrasonicSensor();
    private double rotPerSecondsAt50 = 0.875;
    private double rotPerSecondsAt100 = 73/30.0;

    public NxtRobot() {

        getMotors().addObserver(new Observer() {
            @Override
            public void update(Observable o, Object arg) {

                usens.setPosition(new Vector3d(getMotors().getPosition()));
                usens.setOrientation(new Vector3d(getMotors().getOrientation()));

            }
        });

    }

    /**
     *
     * @param name
     * @return
     */
    @Override
    public Sensor getSensor(String name) {

        if (name.equals("usens")) {

            return usens;
        }

        return null;

    }

    @Override
    public Shape getShape() {

        double x = getX();
        double y = getY();

        double a = getAngle();

        double r = 0.08;

        GeneralPath robotShape = new GeneralPath();
        robotShape.append(new Rectangle2D.Double(x - r, y - r, 2 * r, 2 * r), true);

        robotShape.append(new Rectangle2D.Double(x + r * 9 / 8, y - r, r * 2 / 8, 2 * r), false);
        robotShape.append(new Rectangle2D.Double(x - r / 2, y - r * 11 / 8, r, r * 2 / 8), false);
        robotShape.append(new Rectangle2D.Double(x - r / 2, y + r * 9 / 8, r, r * 2 / 8), false);

        GeneralPath p = new GeneralPath();

        PathIterator pathIterator = robotShape.getPathIterator(AffineTransform.getRotateInstance(a, x, y));
        p.append(pathIterator, true);

        return p;

    }

    @Override
    public void setWorld(World w) {

        usens.setWorld(w);
    }

    @Override
    public void setRightMotorSpeed(double speed) {
       
        double rot = computeRot(speed);
        
        super.setRightMotorSpeed(rot); 
    }

    @Override
    public void setLeftMotorSpeed(double speed) {
        
        double rot = computeRot(speed);
        
        super.setLeftMotorSpeed(rot); 
    }

    @Override
    public void setMotorSpeeds(double sl, double sr) {
        
        double rotl= computeRot(sl);
        double rotr = computeRot(sr);
        
        super.setMotorSpeeds(rotl, rotr); 
    }

    private double computeRot(double power) {
        
        // without a load rpm = 160 * power
        
        return 160.0/60.0 * power/100.0;
        
    }
    
    
    
}
